#ifndef ROBOTSTATE_H
#define ROBOTSTATE_H

#include <utility>
#include <vector>
#include <string.h>
#include <mutex>
#include <unordered_map>
#include <memory>
#include "RTDEProtocol.h"

namespace CURCONTROL{

using namespace std;
/*!
* \brief rtde的output数据结构
*/
class RobotState
{
public:
    /*!
   * \brief 节点数据，由它组成整个RobotState
   */
    struct Node{
        unique_ptr<mutex> mtx_{new mutex};// 该节点锁的智能指针
        vector<uint8_t> value_;// 数据
        Node(size_t size = 8){
            value_.resize(size);
        }
    };

    RobotState()
    {
        // 初始化节点数据集
        for(const auto &ele:OUTPUT_VARIABLES){
            string key = ele.name_;
            data_[key] = Node(ele.variableSize_);
        }
    }
public:
    /*!
   * \brief 读取数据
   *
   * \param[in] 元素名称
   * \param[out] 获取的数据引用
   */
    template<typename T>
    bool GetData(const string &name, T& val) {
        auto iter = data_.find(name);
        if(iter == data_.end()){
            return false;
        }
        Node *node = &(*iter).second;
        // 保护该节点数据
        lock_guard<mutex> lk(*node->mtx_);
        memcpy(&val, &node->value_[0], node->value_.size());
        return true;
    }
    /*!
   * \brief 读取数据 (vector模板参数类的重载)
   */
    template<typename T>
    bool GetData(const string &name, std::vector<T>& val) {
        auto iter = data_.find(name);
        if(iter == data_.end()){
            return false;
        }
        Node *node = &(*iter).second;
        val.resize(node->value_.size() / sizeof (T));
        // 保护该节点数据
        lock_guard<mutex> lk(*node->mtx_);
        memcpy(&val[0], &node->value_[0], node->value_.size());
        return true;
    }
    /*!
   * \brief 写入数据
   *
   * \param[in] 元素名称
   * \param[in] 需要写入的数据
   *
   * \returns 写入数据成功返回true
   */
    template<typename T>
    bool SetData(const string &name, const T& val){
        auto iter = data_.find(name);
        if(iter == data_.end()){
            return false;
        }
        Node *node = &(*iter).second;
        if(sizeof (T) != node->value_.size()){
            return false;
        }
        // 保护该节点数据
        lock_guard<mutex> lk(*node->mtx_);
        memcpy(&node->value_[0], &val, node->value_.size());
        return true;
    }
    /*!
   * \brief 写入数据(vector模板参数类的重载)
   */
    template<typename T>
    bool SetData(const string &name, const std::vector<T>& val){
        auto iter = data_.find(name);
        if(iter == data_.end()){
            return false;
        }
        Node *node = &(*iter).second;
        if(val.size() * sizeof (T) != node->value_.size()){
            return false;
        }
        // 保护该节点数据
        lock_guard<mutex> lk(*node->mtx_);
        memcpy(&node->value_[0], &val[0], node->value_.size());
        return true;
    }
private:
    std::unordered_map<string, Node> data_;// 节点数据集
};

}// namespace CURCONTROL
#endif // ROBOTSTATE_H
